Highway Evaluation of Terrain-aided Localization Using Particle Filters

This work evaluates a real-time algorithm to localize a vehicle on a highway in the direction of travel without the use of GPS. The algorithm uses a particle filter to estimate vehicle position along a map of road grade using real-time pitch measurements from an in-vehicle pitch sensor as the input. Experiments over 60 kilometers along Interstate I-80 and US Route 220 in Pennsylvania are used to demonstrate the algorithm, observe the speed of convergence, and evaluate several methods of implementation. The results indicate that the method can localize a vehicle with a position accuracy of 5 meters after traveling about 1 kilometer within the 60 kilometer map. INTRODUCTION For reasons of safety and efficiency, there is a great deal of interest in localizing road vehicles. Today, the Global Positioning System (GPS) serves as the primary means to determine vehicle position. However, due to poor GPS signal reception in some locations, the ease of jamming a GPS signal in battlefield operation, and the need for sensor redundancy in vehicle automation and driver assist applications, there has been great interest in localization technologies independent of GPS. This study directly follows the work done in [1] and the preliminary discussion, methodology, and experimental validation can be found therein. This study extends the previous work of vehicle localization along a one-mile test track to implementation along actual highways with an on-vehicle map of over 60 kilometers in length. The purpose of this paper is to demonstrate the feasibility of the algorithm to localize a vehicle in an un∗Address all correspondence to this author. controlled environment within which the algorithm is meant to be implemented, like an interstate highway, versus a closed test track as shown in [1]. Highway data is perhaps the most challenging demonstration of the algorithm’s potential to localize a vehicle across large terrain maps. Not only is the vehicle traveling very quickly, and thus the wheelbase filtering effect is quite prominent, but the highway roadway surface itself represents the smoothest roadways available and hence the least likely to excite the pitch variations that are used for correlation. Several methods have been used to localize a vehicle without GPS or during short GPS outages including fusion of GPS with odometry [2], inertial measurements, vision [3], laser scans [4], or using a network of beacons [5]. Of particular relevance to this work is the fusion of GPS with map data. A demonstration of this capability was accomplished in real-time [2] where a Kalman filter was used to combine GPS data with odometry measurements. Similar to the work done in [6] where an aircraft’s elevation profile is matched to a digital elevation map, this work demonstrates the use of a terrain map for real-time vehicle localization with the goal to obtain GPS-accurate position resolution of a vehicle’s longitudinal position. Similar terrain-aided applications include missile guidance systems [7] and underwater robotics [8]. This work uses a particle filter, which is gaining wide use for localization [6, 9, 10], tracking [11], and even vehicle localization during GPS outages [12]. In this study it is assumed that the lane of travel has been previously mapped and that on-vehicle storage of the resulting terrain information is available. The first assumption is quite realistic given the large number of ongoing research projects focused on mapping terrain, whereas the second assumption is increasingly valid given the exponentially decreasing costs of data storage and recent integration of similar on-vehicle map databases into commercial products, for example the “TomTom” navigation devices. PARTICLE FILTER Details presented in this section are a summary of the algorithm used in [1] with significant portions repeated here for completeness; however, the parameter values within the algorithm are adjusted from the previous work in order to extend the application to long-range highway data. Particle filters are Monte-Carlo estimators that are known to be quite robust to non-Gaussian variance distributions similar to what would occur in this work due to similarities in the road profile along different segments. A Kalman filter could not be used to estimate the vehicle position because the vehicle can start anywhere along the map, hence the initial probability distribution is uniform whereas a Kalman filter requires a gaussian probability distribution. The algorithm begins by converting the time-dependant data to the spatial domain, or more plainly as a function of distance from the starting point. Other than wheelbase filtering which is dependant on velocity, this removes velocity dependence on the pitch data. A set of N equally weighted and randomly distributed particles are located along the terrain map. The pitch estimate of each particle location is determined from the pitch map; particles that lie between the discretely mapped locations are determined via linear interpolation. The particle filter algorithm is based off of Algorithm 3 in [11] and iterates every dX distance of vehicle travel by repeating the following: First, the position estimates, denoted by X , at interval k are updated from the previous estimate by Xk = Xk−1 +dX +dO (1) where dO is gaussian noise of variance RO, equal to the variance of the odometry measurement. Second, the weights of the position particles are updated by measuring the actual vehicle pitch and comparing it to the particle’s pitch estimate using a standard particle weighting function. The importance density is assumed to be the prior density and the pdf is assumed to be gaussian: qi = exp ( − 1 2·R · (θa−θp,i) 2 ) ∑i=1 ( exp ( − 1 2·R · (θa−θp,i) 2 )) (2) Here R is the measurement noise variance on pitch, θa is the measured pitch, and θp,i is the ith particle’s pitch corresponding to its position along the terrain map. Third, the particles are resampled following Algorithm 2 described in [11] where the number of effective position particles Ne f f is calculated as Ne f f = 1 ∑i=1 ( qi )2 (3) and when Ne f f is below a threshold of NT , the position particles are re-sampled by c = cumsum ( qk ) u1 = rand(1) ·N−1 i = 1 for j = 1...N u j = u1 +( j−1) ·N−1 while u j > ci i = i+1 end Xk j = X k i qj = N −1 end (4) where rand(1) is an evenly distributed random number in [0,1] and cumsum is the cumulative sum. Fourth, the vehicle’s position is estimated as the mean of the position particles. This use of the entire population to characterize the estimate is fairly conservative since the position estimate of the “best” particle is in general far better than that of the population mean. However, for this study on the feasibility of the algorithm itself, convergence of the population to the correct solution is a far better indicator of algorithm performance than is analysis of the best particle estimate. Fifth, as a means of measuring the accuracy of the algorithm the error in the prediction is calculated from the true vehicle position as measured by GPS. Because a driver is not capable of driving over the exact center of the lane, a path error is introduced called the lane-keeping error. An estimate of error between the predicted position to the actual vehicle position would include the lane-keeping error. In order to remove this error the measured vehicle position is projected to the nearest position on the map. The corrected error is calculated as the distance from the predicted position to the corrected measured position as